using System;

namespace ImageProcessing.Common
{
    // note : this was derived from MathNet and recompiled and optimised for wp7, so credit must go to those developers

    public class Matrix
    {
        private readonly double[][] _data;

        private readonly int _rowCount;

        protected int RowCount
        {
            get { return _rowCount; }
        }

        private readonly int _colCount;

        protected int ColCount
        {
            get { return _colCount; }
        }

        public double this[int i, int j]
        {
            get { return _data[i][j]; }
        }

        private Matrix(double[][] arr)
        {
            _data = arr;
            GetRolColCount(arr, out _rowCount, out _colCount);
        }

        private static void GetRolColCount(double[][] arr, out int rows, out int cols)
        {
            rows = arr.Length;
            cols = (rows == 0) ? 0 : arr[0].Length;
        }

        /// <summary>
        /// Create a matrix from a copy of a 2-d array by deep copy
        /// </summary>
        /// <param name="arr"></param>
        /// <returns></returns>
        public static Matrix Create(double[,] arr)
        {
            int rows = arr.GetLength(0);
            int cols = arr.GetLength(1);

            var newData = new double[rows][];

            for (int i = 0; i < rows; i++)
            {
                var col = new double[cols];
                for (int j = 0; j < cols; j++)
                    col[j] = arr[i, j];

                newData[i] = col;
            }

            return new Matrix(newData);
        }

        /// <summary>
        /// create an emtpty matrix to the size requested
        /// </summary>
        /// <param name="rows"></param>
        /// <param name="cols"></param>
        /// <returns></returns>
        public static double[][] CreateMatrixData(int rows, int cols)
        {
            var data = new double[rows][];
            for (int i = 0; i < rows; i++)
                data[i] = new double[cols];

            return data;
        }


        public Matrix Inverse()
        {
            if (_rowCount >= _colCount)
            {
                return Solve(Identity(_rowCount, _rowCount));
            }

            throw new NotImplementedException();
        }

        private Matrix Identity(int rowCount, int colCount)
        {
            double[][] data = new double[rowCount][];
            for (int i = 0; i < rowCount; i++)
            {
                double[] col = new double[colCount];
                if (i < colCount)
                {
                    col[i] = 1.0;
                }

                data[i] = col;
            }

            return new Matrix(data);
        }

        private Matrix Solve(Matrix matrix)
        {
            if (matrix == null || matrix.RowCount != _rowCount)
                throw new ArgumentException();

            // square
            if (_rowCount == _colCount)
                return SolveSqaure(matrix);

            throw new NotImplementedException();
        }

        private Matrix SolveSqaure(Matrix matrix)
        {

            double[][] lu;
            var piviotPoint = CalcLUDecomposition(out lu, matrix);

            int nx = matrix.ColCount;
            Matrix result = matrix.GetMatrix(piviotPoint, 0, nx - 1);
            double[][] x = result;

            // Solve L*Y = B
            for (int k = 0; k < _colCount; k++)
                for (int i = k + 1; i < _colCount; i++)
                    for (int j = 0; j < nx; j++)
                        x[i][j] -= x[k][j] * lu[i][k];

            // solve U*X = Y
            for (int k = _colCount - 1; k >= 0; k--)
            {
                for (int j = 0; j < nx; j++)
                    x[k][j] /= lu[k][k];

                for (int i = 0; i < k; i++)
                    for (int j = 0; j < nx; j++)
                        x[i][j] -= x[k][j] * lu[i][k];
            }

            return result;
        }

        private int[] CalcLUDecomposition(out double[][] lu, Matrix matrix)
        {
            lu = new Matrix(CloneMatrix(_data));
            var rowCount = matrix.RowCount;
            var columnCount = matrix.ColCount;

            var pivoitPoint = new int[rowCount];
            for (int i = 0; i < rowCount; i++)
            {
                pivoitPoint[i] = i;
            }

            int pivsign = 1;

            var LUcolj = new double[rowCount];

            /* Outer loop */
            for (int j = 0; j < columnCount; j++)
            {
                /* Make a copy of the j-th column to localize references */
                for (int i = 0; i < LUcolj.Length; i++)
                {
                    LUcolj[i] = lu[i][j];
                }

                /* Apply previous transformations */
                for (int i = 0; i < LUcolj.Length; i++)
                {
                    /* Most of the time is spent in the following dot product */
                    int kmax = Math.Min(i, j);
                    double s = 0.0;
                    for (int k = 0; k < kmax; k++)
                    {
                        s += lu[i][k] * LUcolj[k];
                    }

                    lu[i][j] = LUcolj[i] -= s;
                }

                /* Find pivot and exchange if necessary */
                int p = j;

                for (int i = j + 1; i < LUcolj.Length; i++)
                {
                    if (Math.Abs(LUcolj[i]) > Math.Abs(LUcolj[p]))
                    {
                        p = i;
                    }
                }

                if (p != j)
                {
                    for (int k = 0; k < columnCount; k++)
                    {
                        double t = lu[p][k];
                        lu[p][k] = lu[j][k];
                        lu[j][k] = t;
                    }

                    int k2 = pivoitPoint[p];
                    pivoitPoint[p] = pivoitPoint[j];
                    pivoitPoint[j] = k2;

                    pivsign = -pivsign;
                }

                /* Compute multipliers */
                if ((j < rowCount) && (lu[j][j] != 0.0))
                {
                    for (int i = j + 1; i < rowCount; i++)
                    {
                        lu[i][j] /= lu[j][j];
                    }
                }
            }
            return pivoitPoint;
        }

        private Matrix GetMatrix(int[] pivoitPoint, int colStart, int colEnd)
        {
            var newData = CreateMatrixData(pivoitPoint.Length, colEnd - colStart + 1);
            for (int i = 0; i < pivoitPoint.Length; i++)
                for (int j = colStart; j <= colEnd; j++)
                    newData[i][j - colStart] = _data[pivoitPoint[i]][j];


            return new Matrix(newData);
        }

        private static double[][] CloneMatrix(double[][] data)
        {
            int rows, cols;
            GetRolColCount(data, out rows, out cols);

            var newData = new double[rows][];
            for (int i = 0; i < rows; i++)
            {
                var col = new double[cols];
                for (int j = 0; j < cols; j++)
                    col[j] = data[i][j];

                newData[i] = col;
            }

            return newData;
        }


        public static Matrix operator *(Matrix m1, Matrix m2)
        {
            if (m1 == null || m2 == null || m1.RowCount != m2.RowCount)
                throw new ArgumentException();

            return m1.Multiply(m2);
        }

        /// <summary>
        /// Linear Multiply A*B
        /// </summary>
        /// <param name="b"></param>
        /// <returns></returns>
        public Matrix Multiply(Matrix b)
        {
            double[][] newData = CreateMatrixData(_rowCount, b.RowCount);
            double[][] bData = b._data;

            // cache col for perf
            for (int j = 0; j < b.ColCount; j++)
            {
                var colB = new double[_colCount];
                for (int k = 0; k < _colCount; k++)
                    colB[k] = bData[k][j];

                // make the line to column product
                for (int i = 0; i < _rowCount; i++)
                {
                    double s = 0;
                    double[] datai = _data[i];
                    var dlength = datai.Length;
                    for (int k = 0; k < dlength; k++)
                        s += datai[k] * colB[k];


                    newData[i][j] = s;
                }

            }

            return new Matrix(newData);
        }

        public static Matrix operator *(double s, Matrix m)
        {
            double[][] newData = CreateMatrixData(m.RowCount, m.ColCount);
            for (int i = 0; i < m.RowCount; i++)
                for (int j = 0; j < m.ColCount; j++)
                    newData[i][j] = s * m[i, j];

            return new Matrix(newData);
        }

        public static implicit operator double[][](Matrix m)
        {
            return m._data;
        }
    }
}